/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/status.h
 * @Description  : 定义AGV导航的工作状态记录类及相关枚举和结构体。 
 *                 包含车辆位置、任务状态、RCS反馈等信息的管理和操作方法。   
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:04:14
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once

#include "geometry.h"
#include "world.h"
#include <agv_msgs/msg/edge_state.hpp>

enum class TagStateType {
    LOCATION_UNKNOWN,
    STATIC_LOCATION,
    DYNAMIC_LOCATION,
    STANDBY_LOCATION
};

enum class TaskStatus {
    UnKnown,
    Start,
    Run,
    NormalFinish,
    AbnormalFinsh
};

// resonponse rcs infor
struct ToRcsFb {
    std::string order_id;
    uint32_t order_update_id;
    std::string node_id;

    ToRcsFb() { Clear(); }

    void Clear() {
        order_id = node_id = "65535";
        order_update_id = 65535;
    }
};
//////////////////////////////////////////////////////////////////////////////
//   The interface of class "CWorkStatusRecord".
class WorkStatusRecord
{
 public:
    // 搜索最佳路径
    enum class SearchBestPathStep { 
        STEP_FAILED = -1,
        STEP_INIT,
        STEP_NEED_SPIN,
        STEP_NEED_S_CURVE
    };

    // Default constructor
    WorkStatusRecord(World *world = NULL);

    // Initialize the object
    void Init(World *world, int level);

    // Clear the status
    void Clear();

    // 寻找当前路径的bits
    uint16_t FindCurBits();

    // Set the vehicle's static location (form #1)
    // BOOL StaticLocation(USHORT uAtNode); //try  not use
    agv_msgs::msg::EdgeState FindCurEdge();

    // Set the vehicle's static location (form #2)
    bool StaticLocation(unsigned short at_node, unsigned short aux_node,
                        bool update_end_node = false, bool use_map_theory_data = true);

    // Set the vehicle's static location (form #3)
    bool StaticLocation(unsigned short at_node, Posture& posture);

    // Set the vehicle's static location
    bool StaticLocation(unsigned short at_node, Angle heading, MoveDir wheel_move_dir,
                        Angle steer, short level);



    // Set the vehicle's dynamic location
    bool DynamicLocation(unsigned short from_node, unsigned short to_node, float abs_progress,
                       Angle heading, MoveDir wheel_move_dir, Angle steer, short level);
    // Get the posture of end node
    // Posture GetEndNodePosture();

    // Set the vehicle's dynamic location
    ProjectData* FindLocByPosture(Posture& posture, Path* ref_path);

    // Set the vehicle's dynamic location
    ProjectData* FindLocByPosture(Posture& posture, float max_dev_x, float max_dev_theta);

    // Assign a new velocity level
    bool SetLevel(short level);

    // Copy the data to a CStatusData structure
    // void GetReportStatusData(TStatusData& StatusData);

    // Get the work status
    // void GetWorkStatus(CBasWorkStatus* pStatus);

    // void SaveToSqlite();

    // load from sqlite
    bool LoadFromSqlite();

    // 计算当前点距离路径中心点的距离
    double Distance(double x1, double y1, double x2, double y2);
    
    // change static fun qf
    bool ComputeDisFromPointToLineCenter(const Posture& posture, Path* path, float& distance);

    // 搜索最佳路径
    bool SearchGoodLazySPath(Posture& posture, Path* path, float& rate, Point& end_point);

    // 搜索最佳路径
    bool SearchGoodSpinPath(const Posture& start, Path* path, float& spin_radius,
                            Angle& start_angle, Angle& end_angle, TurnDir& turn_dir);

    // 搜索最佳路径
    int SearchBestPathByPosture(Posture& posture, float& lazy_s_rate, Angle& angle,
                                Point& end_point, Angle& start_angle, Angle& end_angle,
                                TurnDir& turn_dir, float& spin_radius);

    // 根据当前节点ID获取二维码ID
    int GetQrCodeId(unsigned short node_id);

    // 强制更新当前位姿
    void UpdateCurPos(const Posture& pos);

public:
    bool is_online_;             // Online/Offline
    MoveDir wheel_move_dir_;     // The vehicle's move direction
    unsigned short traj_type_;   // Type of the trajectory
    bool last_is_op_;            // Whether the last code is OP code
    int equipment_status_;

    short mode_;                 // Current work mode
    TagStateType loc_type_;      // 0 - unknown; 1 - static; 2 - dynamic
    unsigned short from_node_;   // The node the vehicle starts from
    unsigned short to_node_;     // The node the vehicle is moving to
    float abs_progress_;         // The progress of the current movement
    float prog_percent_;         // Progress in "percent"
    Posture cur_posture_;        // Current posture m_curPst
    Velocity velocity_;          // Current velocity
    Angle steer_angle_;          // The vehicle's current steer angle
    short level_;                // Current velocity level
    float deviation_progress_;   // X方向上进度误差
    float dev_x_;                // X Deviation to the trajectory
    float dev_theta_;            // Thita deviation to the trajectory
    int nav_level_;              // Navigation level
    unsigned char cur_layer_;    // Navigation layer
    float quality_;              // Quality of laser measurement
    int event_;                  // Current event
    long long task_id_;          // Current task ID
    TaskStatus task_status_;     // Current task status
    unsigned short sub_task_id_; // Current sub-task ID
    unsigned short work_state_;  // Current work state
    bool is_idle_;               // Whether the vehicle is idle
    int rack_follow_up_;         // 是否开启托盘随动
    std::string map_id_;         // 地图ID
    bool to_run_blink_;          // 起步时，blink状态

    // 最近接收到的orderid和updateId和nodeid
    ToRcsFb rcs_fb_;             // 最近接收到的RCS反馈
 private:
    World* world_;
    int user_param_level_;
    ProjectData project_data_;   // Work variable for projection operation
};

